﻿#include "clscalculateab.h"
#include <qfile.h>
#include <qtextstream.h>
#include <qdebug.h>
#include <ActiveQt/qaxobject.h>

#include "ColDefine.h"

#define USE_PCP 1
#include "pcl-1.13/pcl/segmentation/extract_clusters.h"
#if USE_PCP
#include "pcp.h"
#include "pcp-match/fit.h"
#include "pcp-detect/detect_surface.h"
#include "pcp-transform/cloud_trans.h"
#pragma comment(lib, "rindo_pcp.lib")
#else
//#include "fit.h"
//#include <opencv2/core/eigen.hpp>
#endif // USE_PCP

using namespace HalconCpp;
#define mark_quality 4


#if USE_PCP
pcp::common::Plane3D GetPlane3DModel(CloudXYZGray_32 cloud, float fit_step, int fit_max_iter, float fit_prec)
{
	pcp::common::Plane3D res(0, 0, 0, 1);
	pcp::common::Plane3D model_plane = pcp::detect::DetectPlane<float, PointXYZGray_32>(cloud);
	pcp::match::Fit<float, PointXYZGray_32, CloudXYZGray_32> fit;
	fit.SetDeriveStep(fit_step);
	fit.SetMaxIter(fit_max_iter);
	fit.SetPrecision(fit_prec);
	fit.SetModel(model_plane);
	fit.SetPoints(cloud);
	PointXYZ_32 z_axis;
	if (!fit.Compute())
	{
		return res;
	}
	else
	{
		fit.GetModel(&res);

		float sum_1 = 1.0 / std::sqrt(res.A() * res.A() + res.B() * res.B() + res.C() * res.C());
		float A = res.A() * sum_1;
		float B = res.B() * sum_1;
		float C = res.C() * sum_1;
		float D = res.D() * sum_1;
		if (C < 0)
		{
			A = -1 * A;
			B = -1 * B;
			C = -1 * C;
			D = -1 * D;
		}
		res.SetA(A);
		res.SetB(B);
		res.SetC(C);
		res.SetD(D);
	}
	return res;
}
#endif // USE_PCP


bool create_PCL_from_txt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	std::ifstream file(file_path.c_str());  // 打开文件，.c_str将“file_path”转换为c风格字符串
	std::string line;   // 用于临时存储txt文件中的每一行数据。
	pcl::PointXYZ point;    // 用于读取txt文件中的点坐标
	if (file.is_open()) // 判断文件是否被打开
	{
		while (getline(file, line))  // 读取txt文件中的一行，默认遇到“/n”停止
		{
			std::stringstream ss(line); // 将这一行的字符串转换s为输入流，以便提取数据
			ss >> point.x;
			ss >> point.y;
			ss >> point.z;
			cloud->push_back(point);    // 将读取的数据添加进cloud中
		}
		file.close();   // 关闭文件流
		return true;
	}
	else    // 错误处理
	{
		//std::cout << " 无法打开文件 " << std::endl;
		return false;
	}
}

ClsCalculateAB::ClsCalculateAB()
{
	//Init();
	ClearNewMarkTuple();
}

bool ClsCalculateAB::GetMatrixB_Point(const cv::Mat& cloud, cv::Mat& B, double& minx, double& miny, float dx, float dy)
{
	if (!GetMatrixB_Point(cloud, minx, miny, dx, dy))
		return false;
	try
	{
		HTuple halcon_x0 = HTuple();
		HTuple halcon_y0 = HTuple();
		HTuple halcon_z0 = HTuple();
		for (int t = 0; t < mark_quality; t++)
		{
			halcon_x0[t] = v_mark[t * 3 + 0];
			halcon_y0[t] = v_mark[t * 3 + 1];
			halcon_z0[t] = v_mark[t * 3 + 2];
			if (mNewMarkX[t] == 0 && mNewMarkY[t] == 0 && mNewMarkZ[t] == 0)
			{
				B = cv::Mat::eye(4, 4, CV_32FC1);
				mErrorMsg = "Get matrix B fail!";
				return false;
			}
		}
		HTuple hv_HomMat3D2;
		VectorToHomMat3d("rigid", halcon_x0, halcon_y0, halcon_z0, mNewMarkX, mNewMarkY, mNewMarkZ, &hv_HomMat3D2);
		mErrorMsg = "";
		for (size_t i = 0; i < 4; i++)
		{
			mErrorMsg += "[";
			mErrorMsg += "x:" + QString::number(mNewMarkX[i].D(), 'f', 3) + " ";
			mErrorMsg += "y:" + QString::number(mNewMarkY[i].D(), 'f', 3) + " ";
			mErrorMsg += "z:" + QString::number(mNewMarkZ[i].D(), 'f', 3) + " ";
			mErrorMsg += "]";
		}
		for (int h = 0; h < 3; h++)
		{
			for (int w = 0; w < 4; w++)
			{
				B.at<float>(h, w) = hv_HomMat3D2[4 * h + w].D();
			}
		}
		return true;
	}
	catch (const HException& e)
	{
		mErrorMsg = "";
		mErrorMsg += e.ProcName().TextA();
		mErrorMsg += e.ErrorMessage().TextA();
		return false;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
	catch (...)
	{
		mErrorMsg = "Unknown Error";
		return false;
	}
}

bool ClsCalculateAB::GetMatrixB_Point(const cv::Mat& cloud, double& minx, double& miny, float dx, float dy)
{
	HTuple hv_markSize;
	TupleLength(mNewMarkX, &hv_markSize);
	try
	{
		HTuple hv_Number;
		HObject obj = HObject();
		int height = cloud.rows;
		int width = cloud.cols;
		GenImage1(&obj, "real", width, height, (Hlong)cloud.data);
		HObject ho_RegionThresh, ho_ConnectedRegions, ho_SelectedRegions, ho_RegionFillUp, ho_Circle;
		HTuple hv_Area, hv_Row, hv_Column, hv_Radius, hv_A, hv_B, hv_C, hv_D, hv_AreaC, hv_RowC, hv_ColumnC;
		HTuple hv_Rows, hv_Columns, hv_ObjectModel3DOut, hv_GenParamValue1;
		HObject ho_RegionErosion, ho_RegionDilation, ho_inner, ho_outer, ho_RegionDifference;
		HObject ho_Region_Thresh, ho_RegionIntersection;
		Threshold(obj, &ho_RegionThresh, 0, 0);
		Connection(ho_RegionThresh, &ho_ConnectedRegions);
		SelectShape(ho_ConnectedRegions, &ho_SelectedRegions, (HTuple("area").Append("circularity")),
			"and", (HTuple(mAreaSmall).Append(0.52521)), (HTuple(mAreaBig).Append(1)));
		CountObj(ho_SelectedRegions, &hv_Number);
		//if (hv_Number.D() != 1)return false;
		FillUp(ho_SelectedRegions, &ho_RegionFillUp);
		AreaCenter(ho_RegionFillUp, &hv_AreaC, &hv_RowC, &hv_ColumnC);
		ErosionCircle(ho_RegionFillUp, &ho_RegionErosion, 3);
		DilationCircle(ho_RegionErosion, &ho_RegionDilation, 3);
		SmallestCircle(ho_RegionDilation, &hv_Row, &hv_Column, &hv_Radius);
		GenCircle(&ho_inner, hv_Row, hv_Column, hv_Radius + 5);
		GenCircle(&ho_outer, hv_Row, hv_Column, hv_Radius + 25);
		Difference(ho_outer, ho_inner, &ho_RegionDifference);
		Threshold(obj, &ho_Region_Thresh, 2, 2550000);
		Intersection(ho_Region_Thresh, ho_RegionDifference, &ho_RegionIntersection);
		GetRegionPoints(ho_RegionIntersection, &hv_Rows, &hv_Columns);// hv_Rows_Differ, hv_Columns_Differ;
		int  quality = int(hv_Rows.TupleLength());
		HTuple halcon_x = HTuple();
		HTuple halcon_y = HTuple();
		HTuple halcon_z = HTuple();
		HTuple hv_ObjectModel3D = HTuple();
		for (int s = 0; s < quality; s++)
		{
			halcon_x[s] = minx + hv_Columns[s].D() * dx;
			halcon_y[s] = miny + hv_Rows[s].D() * dy;
			halcon_z[s] = cloud.at<float>(hv_Rows[s].D(), hv_Columns[s].D());
		}
		//拟合
		GenObjectModel3dFromPoints(halcon_x, halcon_y, halcon_z, &hv_ObjectModel3D);
		FitPrimitivesObjectModel3d(hv_ObjectModel3D, "primitive_type", "plane", &hv_ObjectModel3DOut);
		GetObjectModel3dParams(hv_ObjectModel3DOut, "primitive_parameter", &hv_GenParamValue1);
		hv_A = ((const HTuple&)hv_GenParamValue1)[0];
		hv_B = ((const HTuple&)hv_GenParamValue1)[1];
		hv_C = ((const HTuple&)hv_GenParamValue1)[2];
		hv_D = ((const HTuple&)hv_GenParamValue1)[3];
		int row = round(hv_Row.D());
		int col = round(hv_Column.D());
		//存结果
		mNewMarkX[hv_markSize] = minx + col * dx;
		mNewMarkY[hv_markSize] = miny + row * dy;
		mNewMarkZ[hv_markSize] = (-hv_A.D() * mNewMarkX[hv_markSize] - hv_B.D() * mNewMarkY[hv_markSize] + hv_D.D()) / hv_C.D();

		return true;
	}
	catch (const HException& e)
	{
		mNewMarkX[hv_markSize] = 0;
		mNewMarkY[hv_markSize] = 0;
		mNewMarkZ[hv_markSize] = 0;
		mErrorMsg = "";
		mErrorMsg += e.ProcName().TextA();
		mErrorMsg += e.ErrorMessage().TextA();
		return false;
	}
	catch (const cv::Exception& e)
	{
		mNewMarkX[hv_markSize] = 0;
		mNewMarkY[hv_markSize] = 0;
		mNewMarkZ[hv_markSize] = 0;
		mErrorMsg = e.what();
		return false;
	}
	catch (...)
	{
		mNewMarkX[hv_markSize] = 0;
		mNewMarkY[hv_markSize] = 0;
		mNewMarkZ[hv_markSize] = 0;
		mErrorMsg = "Unknown Error";
		return false;
	}
}

void ClsCalculateAB::PosToMat(float x, float y, float z, float a, float b, float c, cv::Mat& matrix)
{
	float theta[3] = {
		a * PI / 180,
		b * PI / 180,
		c * PI / 180 };

	cv::Mat R_x = (cv::Mat_<float>(3, 3) <<
		1, 0, 0,
		0, cos(theta[2]), -sin(theta[2]),
		0, sin(theta[2]), cos(theta[2])
		);

	// Calculate rotation about b axis
	cv::Mat R_y = (cv::Mat_<float>(3, 3) <<
		cos(theta[1]), 0, sin(theta[1]),
		0, 1, 0,
		-sin(theta[1]), 0, cos(theta[1])
		);

	// Calculate rotation about c axis
	cv::Mat R_z = (cv::Mat_<float>(3, 3) <<
		cos(theta[0]), -sin(theta[0]), 0,
		sin(theta[0]), cos(theta[0]), 0,
		0, 0, 1);

	// Combined rotation matrix
	cv::Mat R = R_z * R_y * R_x;

	matrix = cv::Mat::zeros(4, 4, CV_32FC1);
	for (size_t i = 0; i < R.rows; i++)
		for (size_t j = 0; j < R.cols; j++)
			matrix.at<float>(i, j) = R.at<float>(i, j);
	matrix.at<float>(0, 3) = x;
	matrix.at<float>(1, 3) = y;
	matrix.at<float>(2, 3) = z;
	matrix.at<float>(3, 3) = 1;

}

void ClsCalculateAB::MatToPos(const cv::Mat& matrix, QStringList& pos, const QStringList& originPos)
{
	pos = QStringList() << "" << "" << "" << "" << "" << "";

	cv::Mat rotateMatrix = cv::Mat(3, 3, CV_32FC1);
	for (size_t i = 0; i < rotateMatrix.rows; i++)
		for (size_t j = 0; j < rotateMatrix.cols; j++)
			rotateMatrix.at<float>(i, j) = matrix.at<float>(i, j);

	float a, b, c;
	float a1, b1, c1;
#if 0
	float sy = sqrt(rotateMatrix.at<double>(0, 0) * rotateMatrix.at<double>(0, 0) + rotateMatrix.at<double>(1, 0) * rotateMatrix.at<double>(1, 0));

	bool singular = sy < 1e-6; // If

	if (!singular)
	{
		a = atan2(rotateMatrix.at<double>(2, 1), rotateMatrix.at<double>(2, 2));
		b = atan2(-rotateMatrix.at<double>(2, 0), sy);
		c = atan2(rotateMatrix.at<double>(1, 0), rotateMatrix.at<double>(0, 0));
	}
	else
	{
		a = atan2(-rotateMatrix.at<double>(1, 2), rotateMatrix.at<double>(1, 1));
		b = atan2(-rotateMatrix.at<double>(2, 0), sy);
		c = 0;
	}
#else
	if (std::abs(rotateMatrix.at<float>(2, 0)) != 1)
	{
		b = -asin(rotateMatrix.at<float>(2, 0));
		b1 = PI - b;

		c = atan2(rotateMatrix.at<float>(2, 1) / cos(b)
			, rotateMatrix.at<float>(2, 2) / cos(b));
		c1 = atan2(rotateMatrix.at<float>(2, 1) / cos(b1)
			, rotateMatrix.at<float>(2, 2) / cos(b1));

		a = atan2(rotateMatrix.at<float>(1, 0) / cos(b)
			, rotateMatrix.at<float>(0, 0) / cos(b));
		a1 = atan2(rotateMatrix.at<float>(1, 0) / cos(b1)
			, rotateMatrix.at<float>(0, 0) / cos(b1));
	}
	else
	{
		a = 0;
		if (rotateMatrix.at<float>(2, 0) == -1)
		{
			b = PI / 2;
			c = a + atan2(rotateMatrix.at<float>(0, 1)
				, rotateMatrix.at<float>(0, 2));
		}
		else
		{
			b = -PI / 2;
			c = -a + atan2(-rotateMatrix.at<float>(0, 1)
				, -rotateMatrix.at<float>(0, 2));
		}
		a1 = a;
		b1 = b;
		c1 = c;
	}
#endif // 0

	pos[0] = QString::number(matrix.at<float>(0, 3), 'f', 3);
	pos[1] = QString::number(matrix.at<float>(1, 3), 'f', 3);
	pos[2] = QString::number(matrix.at<float>(2, 3), 'f', 3);
	if (originPos.size() == 6)
	{
		float oa, ob, oc;
		oa = originPos.at(3).toFloat() * PI / 180;
		ob = originPos.at(4).toFloat() * PI / 180;
		oc = originPos.at(5).toFloat() * PI / 180;
		float diff1 = std::abs(a - oa) + std::abs(b - ob) + std::abs(c - oc);
		float diff2 = std::abs(a1 - oa) + std::abs(b1 - ob) + std::abs(c1 - oc);
		if (diff1 < diff2)
		{
			pos[3] = QString::number(a * 180 / PI, 'f', 3);
			pos[4] = QString::number(b * 180 / PI, 'f', 3);
			pos[5] = QString::number(c * 180 / PI, 'f', 3);
		}
		else
		{
			pos[3] = QString::number(a1 * 180 / PI, 'f', 3);
			pos[4] = QString::number(b1 * 180 / PI, 'f', 3);
			pos[5] = QString::number(c1 * 180 / PI, 'f', 3);
		}
	}
	else
	{
		pos[3] = QString::number(a * 180 / PI, 'f', 3);
		pos[4] = QString::number(b * 180 / PI, 'f', 3);
		pos[5] = QString::number(c * 180 / PI, 'f', 3);
	}
}

void ClsCalculateAB::MatToPosDiff(const cv::Mat& matrix, QStringList& diffList, const QStringList& originPos)
{
	diffList = QStringList() << "" << "" << "" << "" << "" << "";

	cv::Mat rotateMatrix = cv::Mat(3, 3, CV_32FC1);
	for (size_t i = 0; i < rotateMatrix.rows; i++)
		for (size_t j = 0; j < rotateMatrix.cols; j++)
			rotateMatrix.at<float>(i, j) = matrix.at<float>(i, j);

	float a, b, c;
	float a1, b1, c1;

	if (std::abs(rotateMatrix.at<float>(2, 0)) != 1)
	{
		b = -asin(rotateMatrix.at<float>(2, 0));
		b1 = PI - b;

		c = atan2(rotateMatrix.at<float>(2, 1) / cos(b)
			, rotateMatrix.at<float>(2, 2) / cos(b));
		c1 = atan2(rotateMatrix.at<float>(2, 1) / cos(b1)
			, rotateMatrix.at<float>(2, 2) / cos(b1));

		a = atan2(rotateMatrix.at<float>(1, 0) / cos(b)
			, rotateMatrix.at<float>(0, 0) / cos(b));
		a1 = atan2(rotateMatrix.at<float>(1, 0) / cos(b1)
			, rotateMatrix.at<float>(0, 0) / cos(b1));
	}
	else
	{
		a = 0;
		if (rotateMatrix.at<float>(2, 0) == -1)
		{
			b = PI / 2;
			c = a + atan2(rotateMatrix.at<float>(0, 1)
				, rotateMatrix.at<float>(0, 2));
		}
		else
		{
			b = -PI / 2;
			c = -a + atan2(-rotateMatrix.at<float>(0, 1)
				, -rotateMatrix.at<float>(0, 2));
		}
		a1 = a;
		b1 = b;
		c1 = c;
	}


	if (originPos.size() == 6)
	{
		diffList[0] = QString::number(matrix.at<float>(0, 3) - originPos.at(0).toFloat(), 'f', 3);
		diffList[1] = QString::number(matrix.at<float>(1, 3) - originPos.at(1).toFloat(), 'f', 3);
		diffList[2] = QString::number(matrix.at<float>(2, 3) - originPos.at(2).toFloat(), 'f', 3);

		float oa = originPos.at(3).toFloat() * PI / 180;
		float ob = originPos.at(4).toFloat() * PI / 180;
		float oc = originPos.at(5).toFloat() * PI / 180;

		float diff1 = std::abs(a - oa) + std::abs(b - ob) + std::abs(c - oc);
		float diff2 = std::abs(a1 - oa) + std::abs(b1 - ob) + std::abs(c1 - oc);
		if (diff1 < diff2)
		{
			diffList[3] = QString::number((a - oa) * 180 / PI, 'f', 3);
			diffList[4] = QString::number((b - ob) * 180 / PI, 'f', 3);
			diffList[5] = QString::number((c - oc) * 180 / PI, 'f', 3);
		}
		else
		{
			diffList[3] = QString::number((a1 - oa) * 180 / PI, 'f', 3);
			diffList[4] = QString::number((b1 - ob) * 180 / PI, 'f', 3);
			diffList[5] = QString::number((c1 - oc) * 180 / PI, 'f', 3);
		}
	}
	else
	{
		diffList[0] = QString::number(0);
		diffList[1] = QString::number(0);
		diffList[2] = QString::number(0);
		diffList[3] = QString::number(0);
		diffList[4] = QString::number(0);
		diffList[5] = QString::number(0);
	}
}

bool ClsCalculateAB::PosTrans(const cv::Mat& pos, const cv::Mat& trans, cv::Mat& newpos)
{
	try
	{
		newpos = trans * pos;
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, const cv::Mat& pos, cv::Mat& newcloud, float deltaX, float deltaY, float deltaZ)
{
	try
	{
		newcloud = pos * trans * cloud;
		for (int i = 0; i < newcloud.cols; i++)
		{
			newcloud.at<float>(0, i) += deltaX;
			newcloud.at<float>(1, i) += deltaY;
			newcloud.at<float>(2, i) += deltaZ;
		}
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, const cv::Mat& pos, cv::Mat& newcloud)
{
	try
	{
		newcloud = pos * trans * cloud;
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, cv::Mat& newcloud)
{
	try
	{
		newcloud = trans * cloud;
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::DepthTrans(const cv::Mat& transedXyz, cv::Mat& newDepth, double& minx, double& miny, float dx, float dy)
{
	try
	{
		cv::Mat rowX = transedXyz.row(0);
		cv::Mat rowY = transedXyz.row(1);
		cv::Mat rowZ = transedXyz.row(2);
		double maxvalueX = 0, maxvalueY = 0;
		minx = 0;
		miny = 0;
		cv::minMaxLoc(rowX, &minx, &maxvalueX);
		cv::minMaxLoc(rowY, &miny, &maxvalueY);
		int cols = (int)roundf((maxvalueX - minx) / dx) + 1;
		int rows = (int)roundf((maxvalueY - miny) / dy) + 1;
		newDepth = cv::Mat(rows, cols, CV_32FC1);
		for (int i = 0; i < transedXyz.cols; i++)
		{
			int c = (int)roundf((transedXyz.at<float>(0, i) - minx) / dx) + 1;
			int r = (int)roundf((transedXyz.at<float>(1, i) - miny) / dy) + 1;
			if (r >= 0 && r < rows && c >= 0 && c < cols)
				newDepth.at<float>(r, c) = transedXyz.at<float>(2, i);
		}
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::Sampling(const cv::Mat& cloud, int samplingGap, int rows, int cols, cv::Mat& sampledCloud)
{
	try
	{
		sampledCloud = cv::Mat::zeros(4, (rows / samplingGap + rows % 2) * (cols / samplingGap + cols % 2), CV_32FC1);
		int i = 0;
		for (size_t row = 0; row < rows; row += samplingGap)
		{
			for (int col = 0; col < cols; col += samplingGap)
			{
				sampledCloud.at<float>(0, i) = cloud.at<float>(0, row * cols + col);
				sampledCloud.at<float>(1, i) = cloud.at<float>(1, row * cols + col);
				sampledCloud.at<float>(2, i) = cloud.at<float>(2, row * cols + col);
				sampledCloud.at<float>(3, i) = 1;
				i++;
			}
		}
		return true;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
}

bool ClsCalculateAB::Init()
{
	mRadius = mAlgParam.value("Radius", "190").toInt();
	mSampleB = mAlgParam.value("SampleB", "0.03").toFloat();
	mAreaSmall = mAlgParam.value("AreaSmall", "5165").toInt();
	mAreaBig = mAlgParam.value("AreaBig", "70454").toInt();

	v_mark.clear();
	QString section = "Mark";// 参数区域 可以写死 记得ini得有
	for (int i = 1; i <= mark_quality; i++)
	{
		QString key_x = QString("TransBX%1").arg(i);
		QString key_y = QString("TransBY%1").arg(i);
		QString key_z = QString("TransBZ%1").arg(i);
		v_mark.append(mAlgParam.value(key_x).toFloat());
		v_mark.append(mAlgParam.value(key_y).toFloat());
		v_mark.append(mAlgParam.value(key_z).toFloat());
	}
	return true;
}

void ClsCalculateAB::SetParams(const QMap<QString, QString>& param)
{
	mAlgParam = param;
}

void ClsCalculateAB::ClearNewMarkTuple()
{
	mNewMarkX = HTuple();
	mNewMarkY = HTuple();
	mNewMarkZ = HTuple();
}

QString ClsCalculateAB::GetLastErrorMsg() const
{
	return mErrorMsg;
}

cv::Mat ClsCalculateAB::normalspacesample(cv::Mat cloud_Mat, int width, int height, int k, float sample_ratio, int kBinNum)
{
	auto cloud = CvMatToPclPointXYZ(cloud_Mat, width, height);
	//pcl::PointCloud<pcl::PointXYZ>::Ptr nanRemoved(new pcl::PointCloud<pcl::PointXYZ>);
	//pcl::Indices indices;
	//pcl::removeNaNFromPointCloud(*cloud, *nanRemoved, indices);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);//创建用于最近邻搜索的KD-Tree
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);//存储输出数据
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normalEstimation;//创建法线估计向量
	normalEstimation.setInputCloud(cloud);                                                                                                        //输入点云
	normalEstimation.setKSearch(k);                                                                                                                        //使用当前点周围最近的个数
	normalEstimation.setSearchMethod(tree);
	normalEstimation.compute(*cloud_normals);//计算法线

	//通过concatenateFields函数将point和normal组合起来形成PointNormal点云数据
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normal(new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields(*cloud, *cloud_normals, *cloud_with_normal);
	//pcl::io::savePCDFile("cloud_with_normal1.pcd", *cloud_with_normal);

	// 创建法向空间采样（模板）类对象
	pcl::NormalSpaceSampling<pcl::PointNormal, pcl::Normal> nss;
	// 设置xyz三个法向空间的分类组数，此处设置为一致，根据具体场景可以调整
	//const int kBinNum = 18;
	nss.setBins(kBinNum, kBinNum, kBinNum);
	// 如果传入的是有序点云，此处可以尝试设置为true
	nss.setKeepOrganized(false);
	// 设置随机种子，这样可以保证同样的输入可以得到同样的结果，便于debug分析
	nss.setSeed(200);   // random seed
	// 传入待采样的点云数据
	nss.setInputCloud(cloud_with_normal);
	// 传入用于采样分析的法线数据，需与传入点云数据一一对应
	nss.setNormals(cloud_normals);
	// 设置采样总数，即目标点云的总数据量
	//const float kSampleRatio = 0.1f;
	nss.setSample(cloud_with_normal->size() * sample_ratio);
	// 执行采样并带出采样结果
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normal_sampled(new pcl::PointCloud<pcl::PointNormal>);
	nss.filter(*cloud_with_normal_sampled);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	cloud_filtered->width = cloud_with_normal_sampled->width;
	cloud_filtered->height = cloud_with_normal_sampled->height;
	cloud_filtered->points.resize(cloud_filtered->width * cloud_filtered->height);
	//pcl::copyPointCloud(*cloud_with_normal_sampled, *cloud_filtered);
	pcl::copyPointCloud(*cloud_with_normal_sampled, *cloud_filtered);
	//pcl::io::savePCDFile("cloud_with_normal.pcd", *cloud_filtered);
	//return cloud_filtered;
	return PclPointXYZToCvMat(cloud_filtered);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ClsCalculateAB::CvMatToPclPointXYZ(const cv::Mat& cloud, int width, int height)
{
	//int new_size = 0;
	//for (int i = 0; i < cloud.cols; i++)
	//{
	//	if (cloud.at<float>(2, i) == cloud.at<float>(2, i))
	//		new_size++;
	//}
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//        CloudXYZ_32转pcl点云
	pcl_cloud->width = cloud.cols/*new_size*/;
	pcl_cloud->height = 1;
	pcl_cloud->is_dense = false;
	pcl_cloud->points.resize(pcl_cloud->width * pcl_cloud->height);
	int j = 0;
	for (int i = 0; i < pcl_cloud->points.size(); ++i)
	{
		//if (cloud.at<float>(2, j) != cloud.at<float>(2, j))
		//{
		//	i--;
		//}
		//else
		//{
		pcl_cloud->points[i].x = cloud.at<float>(0, j);
		pcl_cloud->points[i].y = cloud.at<float>(1, j);
		pcl_cloud->points[i].z = cloud.at<float>(2, j);
		//}
		j++;
	}
	return pcl_cloud;
}

cv::Mat ClsCalculateAB::PclPointXYZToCvMat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	cv::Mat cloud_mat = cv::Mat(4, cloud->height * cloud->width, CV_32FC1);
	for (int i = 0; i < cloud->points.size(); ++i)
	{

		cloud_mat.at<float>(0, i) = cloud->points[i].x;
		cloud_mat.at<float>(1, i) = cloud->points[i].y;
		cloud_mat.at<float>(2, i) = cloud->points[i].z;
		cloud_mat.at<float>(3, i) = 1;
	}
	return cloud_mat;
}

QString ClsCalculateAB::ReadValue(const QString& strSection, const QString& Key, const QString& str_INIPath)
{
	QSettings settings(str_INIPath, QSettings::Format::IniFormat);
	settings.beginGroup(strSection);
	QString str{ "0" };
	if (settings.childKeys().contains(Key))
	{
		str = settings.value(Key, "0").toString();
	}
	settings.endGroup();
	return  str;
}

bool ClsCalculateAB::FirstCheck(const cv::Mat& xyz, const cv::Mat& gray, const cv::Mat& handEye, const cv::Mat& posMat)
{
	cv::Mat cornerPoints = cv::Mat(11 * 8, 2, CV_32FC1);
	//找角点
	if (!FindCornerPoints(gray, cornerPoints))
	{
		mErrorMsg = "can not find corner points";
		return false;
	}

	cv::Mat rowX = xyz.row(0);
	cv::Mat rowY = xyz.row(1);
	cv::Mat rowZ = xyz.row(2);
	double maxX = 0, maxY = 0, minx = 0, miny = 0;
	cv::minMaxLoc(rowX, &minx, &maxX);
	cv::minMaxLoc(rowY, &miny, &maxY);

	std::vector<cv::Point2f> pts_cv;//存放点在图像上的位置值（坐标值）
	std::vector<CalibratePos> pts;
	for (int i = 0; i < cornerPoints.rows; ++i)
	{
		cv::Point2f point(cornerPoints.at<float>(i, 0), cornerPoints.at<float>(i, 1));
		pts_cv.push_back(point);
		CalibratePos pt;
		pt._deltaX = point.x * 0.1 + minx;
		pt._deltaY = point.y * 0.1 + miny;
		pt._deltaZ = 0;
		pts.push_back(pt);
	}
	////以下代码优化Z值，用提取出的角点的XY面上的数据求一个圆形ROI，再根据三维信息拟合出一个平面，根据平面重新计算角点的Z值
	cv::RotatedRect rorect = cv::minAreaRect(pts_cv);  //最小外接矩形
	cv::Point2f rect[4];
	rorect.points(rect);  //rect是一个输出参数，拿到最小外接矩形的四个地点
	cv::Point2f center = rorect.center;  //中心点坐标
	float radius = sqrtf((rect[0].x - center.x) * (rect[0].x - center.x) + (rect[0].y - center.y) * (rect[0].y - center.y)) / 1.414f;
	cv::Mat roi_mat = cv::Mat::zeros(gray.rows, gray.cols, CV_8UC1);//初始化了一个图像那么大的Mat
	cv::circle(roi_mat, rorect.center, radius, cv::Scalar(255, 255, 255), -1);

#if !USE_PCP
	cv::Mat cloud;
	for (size_t i = 0; i < gray.rows; i += 20)
	{
		for (size_t j = 0; j < gray.cols; j += 20)
		{
			int cloud_index = i * gray.cols + j;
			if (!std::isnan(xyz.at<float>(2, cloud_index)))
			{
				if (roi_mat.at<uchar>(i, j) > 0)
				{
					cv::Mat pt = (cv::Mat_<float>(1, 3)
						<< xyz.at<float>(0, cloud_index)
						, xyz.at<float>(1, cloud_index)
						, xyz.at<float>(2, cloud_index));
					cloud.push_back(pt);
				}
			}
		}
	}

	float A, B, C, D;
	if (!GetPlane(cloud, &A, &B, &C, &D))
	{
		mErrorMsg = "get plane fail";
		return false;
	}

	for (size_t k = 0; k < pts.size(); k++)
	{
		float x = pts[k]._deltaX;
		float y = pts[k]._deltaY;
		pts[k]._deltaZ = -(A * x + B * y + D) / C;
	}
	////以下代码优化Z值，用提取出的角点的XY面上的数据求一个圆形ROI，再根据三维信息拟合出一个平面，根据平面重新计算角点的Z值
		//求最边缘的四个角点
	std::vector<CalibratePos> cornerPointsVec;
	if (!GetEdgeCornerPoint(pts, cornerPointsVec))
	{
		mErrorMsg = "can not find 4 edge corner points";
		return false;
	}

	cv::Mat cloud1;
	for (size_t i = 0; i < cornerPointsVec.size(); i++)
	{
		cv::Mat corner_point = (cv::Mat_<float>(1, 4)
			<< cornerPointsVec[i]._deltaX, cornerPointsVec[i]._deltaY, cornerPointsVec[i]._deltaZ, 1);
		cloud1.push_back(corner_point);
	}
	cv::Mat hand_eye_trans = posMat * handEye; //拍照姿态*标定结果
	cv::Mat resMat = TransformWithOrigin(cloud1, hand_eye_trans);
#else
	pcp::common::CloudXYZGray_32 cloud;
	for (size_t i = 0; i < gray.rows; i += 20)
	{
		for (size_t j = 0; j < gray.cols; j += 20)
		{
			int cloud_index = i * gray.cols + j;
			if (!std::isnan(xyz.at<float>(2, cloud_index)))
			{
				if (roi_mat.at<uchar>(i, j) > 0)
				{
					PointXYZGray_32 pt;
					pt.SetX(xyz.at<float>(0, cloud_index));
					pt.SetY(xyz.at<float>(1, cloud_index));
					pt.SetZ(xyz.at<float>(2, cloud_index));
					cloud.Push_back(pt);
				}
			}
		}
	}
	UpdateBounding(&cloud);
	pcp::common::Plane3D plane = GetPlane3DModel(cloud, 0.1, 50, 1e-8);
	for (size_t k = 0; k < pts.size(); k++)
	{
		pts[k]._deltaZ = -(plane.A() * pts[k]._deltaX + plane.B() * pts[k]._deltaY + plane.D()) / plane.C();
	}

	std::vector<CalibratePos> cornerPointsVec;
	if (!GetEdgeCornerPoint(pts, cornerPointsVec))
	{
		mErrorMsg = "can not find 4 edge corner points";
		return false;
	}
	CloudXYZ_32 cloud1;
	for (size_t i = 0; i < cornerPointsVec.size(); i++)
	{
		PointXYZ_32 corner_point(cornerPointsVec[i]._deltaX, cornerPointsVec[i]._deltaY, cornerPointsVec[i]._deltaZ);
		cloud1.Push_back(corner_point);
	}

	cv::Mat hand_eye_trans = posMat * handEye; //拍照姿态*标定结果
	CloudXYZ_32 cloud_base_robot = pcp::transform::TransformWithOrigin<float, PointXYZ_32, CloudXYZ_32>(cloud1, hand_eye_trans);

	float diff = std::max({
		std::abs(cloud_base_robot[0].X() - mAlgParam.value("FirstCheckX1").toFloat()),
		std::abs(cloud_base_robot[0].Y() - mAlgParam.value("FirstCheckY1").toFloat()),
		std::abs(cloud_base_robot[0].Z() - mAlgParam.value("FirstCheckZ1").toFloat()),

		std::abs(cloud_base_robot[1].X() - mAlgParam.value("FirstCheckX2").toFloat()),
		std::abs(cloud_base_robot[1].Y() - mAlgParam.value("FirstCheckY2").toFloat()),
		std::abs(cloud_base_robot[1].Z() - mAlgParam.value("FirstCheckZ2").toFloat()),

		std::abs(cloud_base_robot[2].X() - mAlgParam.value("FirstCheckX3").toFloat()),
		std::abs(cloud_base_robot[2].Y() - mAlgParam.value("FirstCheckY3").toFloat()),
		std::abs(cloud_base_robot[2].Z() - mAlgParam.value("FirstCheckZ3").toFloat()),

		std::abs(cloud_base_robot[3].X() - mAlgParam.value("FirstCheckX4").toFloat()),
		std::abs(cloud_base_robot[3].Y() - mAlgParam.value("FirstCheckY4").toFloat()),
		std::abs(cloud_base_robot[3].Z() - mAlgParam.value("FirstCheckZ4").toFloat()) });
	if (diff > mAlgParam.value("FirstCheckDiffVar").toFloat())
	{
		mErrorMsg = "diff >" + mAlgParam.value("FirstCheckDiffVar");
		return false;
	}
	else
	{
		return true;
	}
#endif // 0
}

void ClsCalculateAB::TestAlg(const cv::Mat& cloud, cv::Mat& out)
{
#if 0
	try
	{
		HTuple tuple_x = HTuple();
		HTuple tuple_y = HTuple();
		HTuple tuple_z = HTuple();
		for (int i = 0; i < cloud.cols; i++)
		{
			if (cloud.at<float>(2, i) != cloud.at<float>(2, i))
			{
				continue;
			}
			tuple_x[i] = cloud.at<float>(0, i);
			tuple_y[i] = cloud.at<float>(1, i);
			tuple_z[i] = cloud.at<float>(2, i);
		}

		HTuple hv_ObjectModel3D1, ObjectModel3DConnected, ObjectModel3DSelected, UnionObjectModel3D;
		HTuple hv_smooth_x, hv_smooth_y, hv_smooth_z;
		GenObjectModel3dFromPoints(tuple_x, tuple_y, tuple_z, &hv_ObjectModel3D1);
		ConnectionObjectModel3d(hv_ObjectModel3D1, "distance_3d", 0.45, &ObjectModel3DConnected);
		SelectObjectModel3d(ObjectModel3DConnected, "num_points", "and", 10, 9999999, &ObjectModel3DSelected);
		UnionObjectModel3d(ObjectModel3DSelected, "points_surface", &UnionObjectModel3D);
		//WriteObjectModel3d(UnionObjectModel3D, "ply", "D:/ampackData/mls_kNN60_0520conn.ply", HTuple(), HTuple());
		GetObjectModel3dParams(UnionObjectModel3D, "point_coord_x", &hv_smooth_x);
		GetObjectModel3dParams(UnionObjectModel3D, "point_coord_y", &hv_smooth_y);
		GetObjectModel3dParams(UnionObjectModel3D, "point_coord_z", &hv_smooth_z);

		HTuple hv_lenth;
		TupleLength(hv_smooth_x, &hv_lenth);
		cv::Mat out(4, hv_lenth[0].D(), CV_32FC1);
		for (size_t i = 0; i < out.cols; i++)
		{
			out.at<float>(0, i) = hv_smooth_x[i].D();
			out.at<float>(1, i) = hv_smooth_y[i].D();
			out.at<float>(2, i) = hv_smooth_z[i].D();
			out.at<float>(3, i) = 1;
		}
	}
	catch (const HException& e)
	{
		mErrorMsg = "";
		mErrorMsg += e.ProcName().TextA();
		mErrorMsg += e.ErrorMessage().TextA();
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
	}
	catch (...)
	{
		mErrorMsg = "Unknown Error";
	}
#endif
}

void ClsCalculateAB::EuclideanConnection(const cv::Mat& cloud, cv::Mat& out)
{
	try
	{
		auto cloudpcl = CvMatToPclPointXYZ(cloud, 0, 0);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_nan(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::Indices indice;
		pcl::removeNaNFromPointCloud(*cloudpcl, *cloud_without_nan, indice);

		// 创建KdTreea对象用于点云搜索
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
		tree->setInputCloud(cloud_without_nan);

		std::vector<pcl::PointIndices> cluster_indices;  //创建索引对象向量，用于存储不同聚类结果的点云索引

		pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;  //创建欧式聚类对象
		ec.setClusterTolerance(0.7); // 设置距离阈值为13cm。点与点之间小于这个距离阈值视为一类
		ec.setMinClusterSize(12);  //设置聚类最少点数
		ec.setMaxClusterSize(50000000);  //设置聚类最大点数
		ec.setSearchMethod(tree);  //输入点云搜索方法
		ec.setInputCloud(cloud_without_nan);
		ec.extract(cluster_indices);  //开始聚类

		//将聚类结果合成一幅点云，方便后面对比显示
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (auto it = cluster_indices.begin(); it != cluster_indices.end(); ++it)  //循环一次就是一个聚类结果
		{
			for (auto pit = it->indices.begin(); pit != it->indices.end(); ++pit)
				cloud_cluster->push_back((*cloud_without_nan)[*pit]);  //每个聚类结果都装到cloud_cluster这幅点云里
		}
		cloud_cluster->width = cloud_cluster->size();  //点云尺寸的明确，存储点云前需要明确尺寸大小
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		out = PclPointXYZToCvMat(cloud_cluster);
	}
	catch (const pcl::PCLException& e)
	{
		
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
	}
	catch (...)
	{
		mErrorMsg = "Unknown Error";
	}
}

void ClsCalculateAB::OutlineRemoval(const cv::Mat& cloud, cv::Mat& out,int x, int y)
{
	auto cloudpcl = CvMatToPclPointXYZ(cloud, 0, 0);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_nan(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::Indices indice;
	pcl::removeNaNFromPointCloud(*cloudpcl, *cloud_without_nan, indice);

	pcl::PointCloud<pcl::PointXYZ>::Ptr scene_roi_res(new pcl::PointCloud<pcl::PointXYZ>);
	// 填充点云数据...
	// 创建滤波器对象
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_without_nan);
	sor.setMeanK(x);  // 设置在每个点周围的K个点中计算平均
	sor.setStddevMulThresh(y);  // 设置标准差乘以阈值
	sor.filter(*scene_roi_res);  // 执行滤波处理

	out = PclPointXYZToCvMat(scene_roi_res);
}

bool ClsCalculateAB::Smooth(const cv::Mat& cloud, cv::Mat& out)
{
	try
	{
		HTuple tuple_x = HTuple();
		HTuple tuple_y = HTuple();
		HTuple tuple_z = HTuple();
		for (int i = 0; i < cloud.cols; i++)
		{
			if (cloud.at<float>(2, i) != cloud.at<float>(2, i))
			{
				continue;
			}
			tuple_x[i] = cloud.at<float>(0, i);
			tuple_y[i] = cloud.at<float>(1, i);
			tuple_z[i] = cloud.at<float>(2, i);
		}

		HTuple hv_ObjectModel3D, hv_ObjectModelSmooth;
		GenObjectModel3dFromPoints(tuple_x, tuple_y, tuple_z, &hv_ObjectModel3D);
		SmoothObjectModel3d(hv_ObjectModel3D, "mls", "mls_kNN", 100, &hv_ObjectModelSmooth);

		HTuple hv_smooth_x, hv_smooth_y, hv_smooth_z;
		GetObjectModel3dParams(hv_ObjectModelSmooth, "point_coord_x", &hv_smooth_x);
		GetObjectModel3dParams(hv_ObjectModelSmooth, "point_coord_y", &hv_smooth_y);
		GetObjectModel3dParams(hv_ObjectModelSmooth, "point_coord_z", &hv_smooth_z);

		HTuple hv_lenth;
		TupleLength(hv_smooth_x, &hv_lenth);
		out = cv::Mat(4, hv_lenth[0].D(), CV_32FC1);
		for (size_t i = 0; i < out.cols; i++)
		{
			out.at<float>(0, i) = hv_smooth_x[i].D();
			out.at<float>(1, i) = hv_smooth_y[i].D();
			out.at<float>(2, i) = hv_smooth_z[i].D();
			out.at<float>(3, i) = 1;
		}
		return true;
	}
	catch (const HException& e)
	{
		mErrorMsg = "";
		mErrorMsg += e.ProcName().TextA();
		mErrorMsg += e.ErrorMessage().TextA();
		return false;
	}
	catch (const cv::Exception& e)
	{
		mErrorMsg = e.what();
		return false;
	}
	catch (...)
	{
		mErrorMsg = "Unknown Error";
		return false;
	}
}
void ClsCalculateAB::AddCalibOffset(const cv::Mat& cloud, cv::Mat& out,float x,float y,float z)
{
		int nMaxZ = -99999;
		int nMinZ = 99999;
		int nMaxX = -99999;
		int nMinX = 99999;
		int nMaxY = -99999;
		int nMinY = 99999;
		for (int i = 0; i < cloud.cols; i++)
		{
			nMaxX = MAX(nMaxX, cloud.at<float>(0, i));
			nMinX = MIN(nMinX, cloud.at<float>(0, i));
			nMaxY = MAX(nMaxY, cloud.at<float>(1, i));
			nMinY = MIN(nMinY, cloud.at<float>(1, i));
			nMaxZ = MAX(nMaxZ, cloud.at<float>(2, i));
			nMinZ = MIN(nMinZ, cloud.at<float>(2, i));
		}
		float x0, y0, z0, x1, y1, z1;
		for (int i = 0; i < cloud.cols; i++)
		{
			bool bIsMaxX = (abs(cloud.at<float>(0, i) - nMaxX) < 2.0);
			bool bIsMinX = (abs(cloud.at<float>(0, i) - nMinX) < 2.0);
			bool bIsMaxY = (abs(cloud.at<float>(1, i) - nMaxY) < 2.0);
			bool bIsMinY = (abs(cloud.at<float>(1, i) - nMinY) < 2.0);
			float zThresh = ((nMaxZ - nMinZ) > 7) ? 2.5 : 0.5;
			float nStep = ((nMaxZ - nMinZ) < 10) ? 6 : 4;
			bool bIsMinZ = (abs(cloud.at<float>(2, i) - nMinZ) < zThresh);
			if( (bIsMaxX && bIsMinX) || (bIsMaxY && bIsMinY) || bIsMinZ)
			{
				if (1 == (rand() % 4))
				{
					x0 = out.at<float>(0, i);
					y0 = out.at<float>(1, i);
					z0 = out.at<float>(2, i);
					out.at<float>(0, i) += ((rand() % 4) * 0.2);
					out.at<float>(1, i) += ((rand() % 4) * 0.2);
					out.at<float>(2, i) += ((rand() % 30) * 0.01);
					x1 = out.at<float>(0, i);
					y1 = out.at<float>(1, i);
					z1 = out.at<float>(2, i);
				}
			}
			else
			{
				out.at<float>(0, i) += 0;
			}
		}
}

bool ClsCalculateAB::ReduceClouPoint(cv::Mat& MatInData, std::vector<float> vfPolypointxy)
{
	if ((vfPolypointxy.size() == 0) || (MatInData.empty()))
	{
		return true;
	}
	if (vfPolypointxy.size() % 2 == 1)
	{
		return false;
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointInData(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointOutData(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointBoundingData(new pcl::PointCloud<pcl::PointXYZ>);
	for (int i = 0; i < MatInData.cols; i++)
	{
		pcl::PointXYZ temp;
		temp.x = MatInData.at<float>(0, i);
		temp.y = MatInData.at<float>(1, i);
		temp.z = MatInData.at<float>(2, i);
		PointInData->push_back(temp);
	}
	for (int i = 0; i < vfPolypointxy.size(); i++)
	{
		pcl::PointXYZ temp;
		temp.x = vfPolypointxy[i * 2 + 0];
		temp.y = vfPolypointxy[i * 2 + 1];
		temp.z = 0;
		PointBoundingData->push_back(temp);
	}
	cv::Mat MResData;
	if (ReduceByPoint3d(PointInData, PointBoundingData, PointOutData))
	{
		MatInData = PclPointXYZToCvMat(PointOutData);
	}
	return true;
}

bool ClsCalculateAB::ReduceClouPoint(cv::Mat& MatInData, std::string boundingPCLPath)
{
	if ((boundingPCLPath.length() == 0) || (MatInData.empty()))
	{
		return false;
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointInData(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointOutData(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr PointBoundingData(new pcl::PointCloud<pcl::PointXYZ>);
	if (!create_PCL_from_txt(boundingPCLPath, PointBoundingData))
	{
		return false; //输入路径读取点云失败，未执行裁剪
	}

	//检查裁剪模板是否为平面，平面无法凸体裁剪
	float fMax_x = -999999, fMin_x = 999999;
	float fMax_y = -999999, fMin_y = 999999;
	float fMax_z = -999999, fMin_z = 999999;
	for (auto&  point: PointBoundingData->points) { 
		fMax_x = MAX(fMax_x, point.x);
		fMin_x = MIN(fMin_x, point.x);
		fMax_y = MAX(fMax_y, point.y);
		fMin_y = MIN(fMin_y, point.y);
		fMax_z = MAX(fMax_z, point.z);
		fMin_z = MIN(fMin_z, point.z);
	}
	if ((fMax_x - fMin_x) < 0.5)
	{
		for (int i = 0; i < PointBoundingData->points.size();i++)
		{
			PointBoundingData->points[i].x = PointBoundingData->points[i].x + (i % 2 == 0 ? 20.0 : -20.0);
		}
	}
	if ((fMax_y - fMin_y) < 0.5)
	{
		for (int i = 0; i < PointBoundingData->points.size(); i++)
		{
			PointBoundingData->points[i].y = PointBoundingData->points[i].y + (i % 2 == 0 ? 20.0 : -20.0);
		}
	}
	if ((fMax_z - fMin_z) < 0.5)
	{
		for (int i = 0; i < PointBoundingData->points.size(); i++)
		{
			PointBoundingData->points[i].z = PointBoundingData->points[i].z + (i % 2 == 0 ? 20.0 : -20.0);
		}
	}

	for (int i = 0; i < MatInData.cols; i++)
	{
		pcl::PointXYZ temp;
		temp.x = MatInData.at<float>(0, i);
		temp.y = MatInData.at<float>(1, i);
		temp.z = MatInData.at<float>(2, i);
		PointInData->push_back(temp);
	}

	cv::Mat MResData;
	if (ReduceByPoint3d(PointInData, PointBoundingData, PointOutData))
	{
		MatInData = PclPointXYZToCvMat(PointOutData);
		return true;
	}
	else
	{
		return false;
	}
}

bool ClsCalculateAB::ReduceByPoint3d(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> reducePoint, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> result)
{
	//source 被裁剪点云      reducePoint 矩形框或者多边形框以点云的形式输入   result 保存结果
	if (source == nullptr) return false;
	//if (reducePoint->points.size() < 2) return false;
	//pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_Filtered(new pcl::PointCloud<pcl::PointXYZ>);
	//for (int i = 0; i < reducePoint->size(); i++)
	//{
	//	pcl::PointXYZ temp;
	//	temp.x = reducePoint->points[i].x;
	//	temp.y = reducePoint->points[i].y;
	//	temp.z = 600;
	//	boundingbox_Filtered->push_back(temp);
	//	temp.z = -600;
	//	boundingbox_Filtered->push_back(temp);

	//}

	pcl::ConvexHull<pcl::PointXYZ> hull;                                                     // 凸包(即凸多边形)
	hull.setInputCloud(reducePoint);  //      boundingbox_Filtered                                             // 输入点云
	hull.setDimension(2);                                                                       // 凸包维度
	std::vector<pcl::Vertices> polygons;                                                        // 保存凸包顶点
	pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>); // 该点云用于描述凸包形状
	hull.reconstruct(*surface_hull, polygons);                                                  // 计算2D凸包
	pcl::CropHull<pcl::PointXYZ> filter;                                                     // CropHull滤波器
	filter.setDim(2);                                                                           // 设置维度
	filter.setInputCloud(source);                                                                // 输入点云
	filter.setHullIndices(polygons);                                                            // 输入封闭多边形的顶点
	filter.setHullCloud(surface_hull);                                                          // 输入封闭多边形的形状
	filter.filter(*result);
	return true;
}


//void ClsCalculateAB::AddCalib(const cv::Mat& cloud, cv::Mat& out, int x, int y, int z)
void ClsCalculateAB::AddCalib(const cv::Mat& cloud, cv::Mat& out, float x, float y, float z)
{
	for (int i = 0; i < cloud.cols; i++)
	{
		out.at<float>(0, i) += x;
		out.at<float>(1, i) += y;
		out.at<float>(2, i) += z;
	}
}

bool ClsCalculateAB::FindCornerPoints(const cv::Mat& gray, cv::Mat& points)
{
	bool cam_find = false;
	int size = 10;
	while (!cam_find)
	{
		if (size > 100)
		{
			break;
		}
		cv::Mat temp;
		cv::Mat temp1;
		cv::medianBlur(gray, temp, 3);  //中值滤波
		cv::GaussianBlur(temp, temp1, cv::Size(3, 3), 3);
		cv::adaptiveThreshold(temp1, temp1, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, (temp.rows / size * 2) + 1, 1);//自适应阈值分割
		cam_find = cv::findChessboardCorners(temp1, cv::Size(11, 8), points, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK /*| CV_CALIB_CB_NORMALIZE_IMAGE*/);
		size += 10;
	}
	if (!cam_find)
	{
		return false;
	}
	std::vector<cv::Point2f> pts_cv;//存放点在图像上的位置值（坐标值）
	//亚像素角点检测
	cv::cornerSubPix(gray, points, cvSize(20, 20), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
	return true;
}

bool ClsCalculateAB::GetPlane(const cv::Mat& cloud, float* A, float* B, float* C, float* D)
{
#if !USE_PCP
	//计算平面
	int cloudsize = *cloud.size.p;
	Eigen::MatrixXd cloud_m = Eigen::MatrixXd(cloudsize, 3);
	Eigen::MatrixXd res = Eigen::MatrixXd::Ones(cloudsize, 1);
	srand(100);
	for (size_t i = 0; i < cloudsize; i++)
	{
		cv::Mat pt = cloud.row((i + rand()) % cloudsize);
		cv::Mat pt2 = cloud.row((i + rand()) % cloudsize);
		float dx = pt2.at<float>(0, 0) - pt.at<float>(0, 0);
		float dy = pt2.at<float>(0, 1) - pt.at<float>(0, 1);
		float dz = pt2.at<float>(0, 2) - pt.at<float>(0, 2);
		float distance = 1.0 / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2) + FLT_MIN);
		cloud_m.row(i) << dx * distance, dy* distance, dz* distance;
	}

	Eigen::MatrixXd n = Eigen::MatrixXd::Zero(3, 1);
	Eigen::MatrixXd temp = (cloud_m.transpose() * cloud_m);
	float a, b, c, d;
	if (abs(temp.determinant()) < FLT_MIN)
	{
		cv::Vec3f pt01(cloud.at<float>(cloudsize - 1, 0) - cloud.at<float>(0, 0), cloud.at<float>(cloudsize - 1, 1) - cloud.at<float>(0, 1), cloud.at<float>(cloudsize - 1, 2) - cloud.at<float>(0, 2));
		cv::Vec3f pt02(cloud.at<float>(cloudsize - 2, 0) - cloud.at<float>(0, 0), cloud.at<float>(cloudsize - 2, 1) - cloud.at<float>(0, 1), cloud.at<float>(cloudsize - 2, 2) - cloud.at<float>(0, 2));
		//PointT normal = pcp::measure::CrossPoint3D(pt01, pt02);
		a = pt01[1] * pt02[2] - pt01[2] * pt02[1];
		b = pt01[2] * pt02[0] - pt01[0] * pt02[2];
		c = pt01[0] * pt02[1] - pt01[1] * pt02[0];
		d = -1 * (a * cloud.at<float>(0, 0) + b * cloud.at<float>(0, 1) + c * cloud.at<float>(0, 2));
	}
	else
	{
		n = temp.inverse() * cloud_m.transpose() * res;

		a = static_cast<float>(n(0, 0));
		b = static_cast<float>(n(1, 0));
		c = static_cast<float>(n(2, 0));
		d = static_cast<float>(-(cloud.at<float>(0, 0) * n(0, 0) + cloud.at<float>(0, 1) * n(1, 0) + cloud.at<float>(0, 2) * n(2, 0)));
	}
	std::vector model{ a,b,c,d };
	Fit fit;
	fit.SetDeriveStep(0.1);
	fit.SetMaxIter(50);
	fit.SetPrecision(1e-8);
	fit.SetModel(model);
	fit.SetPoints(cloud);
	if (!fit.Compute())
	{
		return false;
	}
	else
	{
		std::vector<float> resVec;
		fit.GetModel(resVec);
		float sum_1 = 1.0 / std::sqrt(std::pow(resVec.at(0), 2) + std::pow(resVec.at(1), 2) + std::pow(resVec.at(2), 2));
		*A = sum_1 * resVec.at(0);
		*B = sum_1 * resVec.at(1);
		*C = sum_1 * resVec.at(2);
		*D = sum_1 * resVec.at(3);
		if (C < 0)
		{
			*A = -1 * (*A);
			*B = -1 * (*B);
			*C = -1 * (*C);
			*D = -1 * (*D);
		}
	}
#endif
	return true;
}

bool ClsCalculateAB::GetEdgeCornerPoint(const std::vector<CalibratePos>& corner, std::vector<CalibratePos>& edgeCorner)
{
	int pointIndex2 = 11 - 1; //8
	int pointIndex3 = 11 * 8 - 1; //53
	int pointIndex4 = pointIndex3 - pointIndex2;
	int pointIndex5 = 11 * 8 - 4;//倒数第5个点用来验证
	edgeCorner.push_back(corner[0]);
	edgeCorner.push_back(corner[pointIndex2]);
	edgeCorner.push_back(corner[pointIndex3]);
	edgeCorner.push_back(corner[pointIndex4]);
	edgeCorner.push_back(corner[pointIndex5]);
	for (size_t i = 0; i < edgeCorner.size(); i++)
	{
		//判断是否为nan
		if (edgeCorner[i]._deltaX != edgeCorner[i]._deltaX
			|| edgeCorner[i]._deltaY != edgeCorner[i]._deltaY
			|| edgeCorner[i]._deltaZ != edgeCorner[i]._deltaZ)
		{
			//没找到角点，重新拍照也许能解决问题
			return false;
		}
	}
	return true;
}

cv::Mat ClsCalculateAB::TransformWithOrigin(const cv::Mat& cloud, const cv::Mat& mat)
{
	cv::Mat cloud_result(cloud.rows, cloud.cols, sizeof(float) / 4 + 4);
#if !USE_PCP
	Eigen::Matrix<float, 4, 4> matrix;
	cv::cv2eigen(mat, matrix);
	Eigen::Matrix<float, 3, 3> R = matrix.block(0, 0, 3, 3);
	Eigen::Matrix<float, 3, 1> T = matrix.block(0, 3, 3, 1);

	cv::Mat mat_T = (cv::Mat_<float>(3, 3) << R(0, 0), R(1, 0), R(2, 0), \
		R(0, 1), R(1, 1), R(2, 1), \
		R(0, 2), R(1, 2), R(2, 2));//R转置

	cv::Mat copy_ori_mat = cv::Mat(cloud, cv::Range(0, cloud.rows), cv::Range(0, 3)).clone();  //注意第4列gray不跟着变换 

	cv::Mat mat_add(cloud.rows, 3, sizeof(float) / 4 + 4);
	mat_add.col(0) = T[0];
	mat_add.col(1) = T[1];
	mat_add.col(2) = T[2];

	cv::Mat temp_mat = copy_ori_mat * mat_T;
	temp_mat = temp_mat + mat_add;

	temp_mat.colRange(0, 3).copyTo(cloud_result.colRange(0, 3));
	cloud.colRange(3, cloud.cols).copyTo(cloud_result.colRange(3, cloud.cols));
#endif
	return cloud_result;
}